#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <signal.h>
#include <pthread.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "CBallCircle.h"
#include "CTargetInfo.h"
#include "CVideoFrame.h"
#include "CVideoFrameQueue.h"
#include "CConnectionServer.h"
#include "CGpioLed.h"
#include "CBallDetector.h"
#include "CFrameGrinder.h"

CFrameGrinder frameGrinder;

// Global shutdown flag is set when user typed Ctrl-C
extern bool g_isShutdown;

int main(int, char**)
{
    frameGrinder.init();

    CVideoFrame* pFrame = NULL;
    cv::VideoCapture camera;

    //open the video stream and make sure it's opened
    if (!camera.open(0)) // "0" --> Use default camera (local USB webcam))
    {
        printf("Error opening video stream or file, err = %d\n", errno);
        return -1;
    }
    if (!camera.isOpened())
    {
        printf("Error opening video stream or file, err = %d\n", errno);
        return -1;
    }
    camera.set(CV_CAP_PROP_FRAME_WIDTH, 426); //640    );  //1920); //640);
    camera.set(CV_CAP_PROP_FRAME_HEIGHT,  240 ); //380  // 1080); //360);
    camera.set(CV_CAP_PROP_FPS, 30);

    //cv::Mat frame1 = cv::imread("/home/qlogic/VisionTarget_Ball_45_angle__426x520.jpg", CV_LOAD_IMAGE_COLOR);
    //cv::Mat frame2 = cv::imread("/home/qlogic/VisionTarget_Ball_small_angle__426x330.jpg", CV_LOAD_IMAGE_COLOR);
    //int toggle = 0;
    while (!g_isShutdown)
    {
        if (frameGrinder.safeGetFreeFrame(&pFrame))
        {
            camera >> pFrame->m_frame;
            //usleep(100 * 1000);
            //if (toggle == 0)
           // {
            //    toggle = 1;
            //    pFrame->m_frame = frame1.clone();
           // }
            //else
            //{
            //    toggle = 0;
            //    pFrame->m_frame = frame2.clone();
           // }
            if (!pFrame->m_frame.empty())
            {
                frameGrinder.safeAddTail(pFrame, CVideoFrame::FRAME_QUEUE_WAIT_FOR_BLOB_DETECT);
            }
            else
            {
                printf("Frame is empty\n");
                frameGrinder.safeAddTail(pFrame, CVideoFrame::FRAME_QUEUE_FREE);
            }
            frameGrinder.m_nTasksDone[CFrameGrinder::TASK_DONE_CAMERA]++;
        }
    }
}
